#include <UnitTest++.h>
#include <Graphics/Quaternion.hpp>
#include <Math/Random.hpp>
#include <Math/Vector3.hpp>
#include <Graphics/Rotation.hpp>
using namespace zzz;

SUITE(QuaternionTest)
{
  TEST(MultiplyTest)
  {
    RandomReal<float> r;
    r.SeedFromTime();
    Vector3f v(r.Rand(), r.Rand(), r.Rand());
    v.Normalize();
    Quaternionf q(r.Rand(), r.Rand(), r.Rand(), r.Rand());
    q.Normalize();
    Quaternionf q2(v[0],v[1],v[2],0);

    Quaternionf q3 = q*q2;
    Vector3f v2 = (q*v).XYZ();
    CHECK_EQUAL(q3.XYZ(), v2);
    
  }

  TEST(RotationTest)
  {
    RandomReal<float> r;
    r.SeedFromTime();
    Vector3f v(r.Rand(), r.Rand(), r.Rand());
    Quaternionf q(r.Rand(), r.Rand(), r.Rand(), r.Rand());
    q.Normalize();

    Vector3f v2(q.RotateVector(v));
    Rotation<float> rot(q);
    Vector3f v3(rot * v);
    CHECK_CLOSE(0, v2.DistTo(v3), BIG_EPSILON);
  }

  TEST(BackRotationTest)
  {
    RandomReal<float> r;
    r.SeedFromTime();
    Vector3f v(r.Rand(), r.Rand(), r.Rand());
    Quaternionf q(r.Rand(), r.Rand(), r.Rand(), r.Rand());
    q.Normalize();

    Vector3f v2(q.RotateBackVector(v));
    Rotation<float> rot(q);
    Vector3f v3(rot.Inverted() * v);
    CHECK_CLOSE(0, v2.DistTo(v3), BIG_EPSILON);
  }

  TEST(RotateBackTest)
  {
    RandomReal<float> r;
    r.SeedFromTime();
    Vector3f v(r.Rand(), r.Rand(), r.Rand());
    v.Normalize();
    Quaternionf q(r.Rand(), r.Rand(), r.Rand(), r.Rand());
    q.Normalize();
    
    Vector3f v2(q.RotateBackVector(q.RotateVector(v)));
    CHECK_CLOSE(0, v2.DistTo(v), BIG_EPSILON);
  }

  TEST(QuaternionSlerpTest)
  {
    RandomReal<float> r;
    r.SeedFromTime();
    Vector3f v(r.Rand(), r.Rand(), r.Rand());
    Quaternionf q1(v, 30 * C_D2R);
    Quaternionf q2(v, 50 * C_D2R);
    Quaternionf q = Quaternionf::Slerp(q1, q2, 0.25);
    CHECK_CLOSE(q.Angle(), 35*C_D2R, BIG_EPSILON);
  }
}